#region Namespaces

using System;
using System.Diagnostics;

#endregion

namespace RSEngine.Math
{

    /// <summary>
    ///	Quaternion.
    ///	
    /// Unit quaternions provide a convenient mathematical notation for representing
    /// orientations and rotations of objects in three dimensions. Compared to Euler angles they are simpler to compose and avoid the problem of gimbal lock.
    ///	
    /// More info at :
    /// http://gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
    /// http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
    /// http://www.j3d.org/matrix_faq/matrfaq_latest.html
    /// </summary>
    public struct Quaternion
    {

        #region Variables

        private const float EPSILON = 1e-03f;

        public float w, x, y, z;

        public static readonly Quaternion Identity = new Quaternion(1.0f, 0.0f, 0.0f, 0.0f);
        public static readonly Quaternion Zero = new Quaternion(0.0f, 0.0f, 0.0f, 0.0f);

        private static readonly int[] next = new int[3] { 1, 2, 0 };

        #endregion

        #region Constructors

        /// <summary>
        /// Quaternion constructor.
        /// </summary>
        /// <param name="w">Is the cosine of (theta / 2), where theta is rotation angle.</param>
        /// <param name="x">The axis is directed by the vector (x,y,z)</param>
        /// <param name="y">The axis is directed by the vector (x,y,z)</param>
        /// <param name="z">The axis is directed by the vector (x,y,z)</param>
        public Quaternion(float w, float x, float y, float z)
        {
            this.w = w;
            this.x = x;
            this.y = y;
            this.z = z;
        }

        #endregion

        #region Operator Overloads

        /// <summary>
        /// Used to multiply 2 Quaternions together.
        /// </summary>
        /// <remarks>
        ///	Quaternion multiplication is not communative in most cases.
        ///	i.e. p*q != q*p
        /// </remarks>
        public static Quaternion operator *(Quaternion left, Quaternion right)
        {
            Quaternion q = new Quaternion();

            q.w = left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z;
            q.x = left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y;
            q.y = left.w * right.y + left.y * right.w + left.z * right.x - left.x * right.z;
            q.z = left.w * right.z + left.z * right.w + left.x * right.y - left.y * right.x;

            /*
            return new Quaternion
                (
                left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z,
                left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y,
                left.w * right.y + left.y * right.w + left.z * right.x - left.x * right.z,
                left.w * right.z + left.z * right.w + left.x * right.y - left.y * right.x
                ); */

            return q;
        }

        /// <summary>
        /// 
        /// </summary>
        public static Vector3 operator *(Quaternion quat, Vector3 vector)
        {
            // nVidia SDK implementation
            Vector3 uv, uuv;
            Vector3 qvec = new Vector3(quat.x, quat.y, quat.z);

            uv = qvec.Cross(vector);
            uuv = qvec.Cross(uv);
            uv *= (2.0f * quat.w);
            uuv *= 2.0f;

            return vector + uv + uuv;

            // get the rotation matrix of the Quaternion and multiply it times the vector
            //return quat.ToRotationMatrix() * vector;
        }

        /// <summary>
        /// Used when a float value is multiplied by a Quaternion.
        /// </summary>
        public static Quaternion operator *(float scalar, Quaternion right)
        {
            return new Quaternion(scalar * right.w, scalar * right.x, scalar * right.y, scalar * right.z);
        }

        /// <summary>
        /// Used when a Quaternion is multiplied by a float value.
        /// </summary>
        public static Quaternion operator *(Quaternion left, float scalar)
        {
            return new Quaternion(scalar * left.w, scalar * left.x, scalar * left.y, scalar * left.z);
        }

        /// <summary>
        /// Used when a Quaternion is added to another Quaternion.
        /// </summary>
        public static Quaternion operator +(Quaternion left, Quaternion right)
        {
            return new Quaternion(left.w + right.w, left.x + right.x, left.y + right.y, left.z + right.z);
        }

        public static Quaternion operator -(Quaternion left, Quaternion right)
        {
            return new Quaternion(left.w - right.w, left.x - right.x, left.y - right.y, left.z - right.z);
        }

        /// <summary>
        ///     Negates a Quaternion, which simply returns a new Quaternion
        ///     with all components negated.
        /// </summary>
        /// <param name="right"></param>
        /// <returns></returns>
        public static Quaternion operator -(Quaternion right)
        {
            return new Quaternion(-right.w, -right.x, -right.y, -right.z);
        }

        public static bool operator ==(Quaternion left, Quaternion right)
        {
            return (left.w == right.w && left.x == right.x && left.y == right.y && left.z == right.z);
        }

        public static bool operator !=(Quaternion left, Quaternion right)
        {
            return !(left == right);
        }

        #endregion

        #region Properties

        /// <summary>
        ///	Squared 'length' of this quaternion.
        /// </summary>
        public float Norm
        {
            get
            {
                return x * x + y * y + z * z + w * w;
            }
        }

        /// <summary>
        /// Axis of the quaternion.
        /// </summary>
        public Vector3 Axis
        {
            get
            {
                Vector3 axis = new Vector3(x, y, z);
                axis.Normalize();
                return axis;
            }
        }

        /// <summary>
        /// Local X-axis portion of this rotation.
        /// </summary>
        public Vector3 XAxis
        {
            get
            {
                float fTx = 2.0f * x;
                float fTy = 2.0f * y;
                float fTz = 2.0f * z;
                float fTwy = fTy * w;
                float fTwz = fTz * w;
                float fTxy = fTy * x;
                float fTxz = fTz * x;
                float fTyy = fTy * y;
                float fTzz = fTz * z;

                return new Vector3(1.0f - (fTyy + fTzz), fTxy + fTwz, fTxz - fTwy);
            }
        }

        /// <summary>
        /// Local Y-axis portion of this rotation.
        /// </summary>
        public Vector3 YAxis
        {
            get
            {
                float fTx = 2.0f * x;
                float fTy = 2.0f * y;
                float fTz = 2.0f * z;
                float fTwx = fTx * w;
                float fTwz = fTz * w;
                float fTxx = fTx * x;
                float fTxy = fTy * x;
                float fTyz = fTz * y;
                float fTzz = fTz * z;

                return new Vector3(fTxy - fTwz, 1.0f - (fTxx + fTzz), fTyz + fTwx);
            }
        }

        /// <summary>
        /// Local Z-axis portion of this rotation.
        /// </summary>
        public Vector3 ZAxis
        {
            get
            {
                float fTx = 2.0f * x;
                float fTy = 2.0f * y;
                float fTz = 2.0f * z;
                float fTwx = fTx * w;
                float fTwy = fTy * w;
                float fTxx = fTx * x;
                float fTxz = fTz * x;
                float fTyy = fTy * y;
                float fTyz = fTz * y;

                return new Vector3(fTxz + fTwy, fTyz - fTwx, 1.0f - (fTxx + fTyy));
            }
        }

        public float PitchInDegrees
        {
            get
            {
                return MathHelper.RadiansToDegrees(Pitch);
            }
            set
            {
                Pitch = MathHelper.DegreesToRadians(value);
            }
        }

        public float YawInDegrees
        {
            get
            {
                return MathHelper.RadiansToDegrees(Yaw);
            }
            set
            {
                Yaw = MathHelper.DegreesToRadians(value);
            }
        }

        public float RollInDegrees
        {
            get
            {
                return MathHelper.RadiansToDegrees(Roll);
            }
            set
            {
                Roll = MathHelper.DegreesToRadians(value);
            }
        }

        public float Pitch
        {
            set
            {
                float pitch, yaw, roll;
                ToEulerAngles(out pitch, out yaw, out roll);
                Quaternion q = FromEulerAngles(value, yaw, roll);
                x = q.x;
                y = q.y;
                z = q.z;
                w = q.w;
            }
            get
            {
                float test = x * y + z * w;
                if (System.Math.Abs(test) > 0.499f) // singularity at north and south pole
                    return 0f;
                return (float)System.Math.Atan2(2 * x * w - 2 * y * z, 1 - 2 * x * x - 2 * z * z);
            }
        }

        public float Yaw
        {
            set
            {
                float pitch, yaw, roll;
                ToEulerAngles(out pitch, out yaw, out roll);
                Quaternion q = FromEulerAngles(pitch, value, roll);
                x = q.x;
                y = q.y;
                z = q.z;
                w = q.w;
            }
            get
            {
                float test = x * y + z * w;
                if (System.Math.Abs(test) > 0.499f) // singularity at north and south pole
                    return (float)(System.Math.Sign(test) * 2 * System.Math.Atan2(x, w));
                return (float)System.Math.Atan2(2 * y * w - 2 * x * z, 1 - 2 * y * y - 2 * z * z);
            }
        }

        public float Roll
        {
            set
            {
                float pitch, yaw, roll;
                ToEulerAngles(out pitch, out yaw, out roll);
                Quaternion q = FromEulerAngles(pitch, yaw, value);
                x = q.x;
                y = q.y;
                z = q.z;
                w = q.w;
            }
            get
            {
                float test = x * y + z * w;
                if (System.Math.Abs(test) > 0.499f) // singularity at north and south pole
                    return System.Math.Sign(test) * MathHelper.PI / 2;
                return (float)System.Math.Asin(2 * test);
            }
        }

        public float Angle
        {
            get
            {
                if (this == Quaternion.Identity)
                    return 0;

                float Y = (float)System.Math.Sqrt(((x * x) + (y * y)) + (z * z));
                float X = w;
                if (Y > float.MaxValue)
                {
                    float num = System.Math.Max(System.Math.Abs(x), System.Math.Max(System.Math.Abs(y), System.Math.Abs(z)));
                    float num5 = x / num;
                    float num4 = y / num;
                    float num3 = z / num;
                    Y = (float)System.Math.Sqrt(((num5 * num5) + (num4 * num4)) + (num3 * num3));
                    X = w / num;
                }
                return (float)(System.Math.Atan2(Y, X) * 114.59155902616465);
            }
        }

        /// <summary>
        /// Computes the inverse of a Quaternion.
        /// </summary>
        public Quaternion Inverse
        {
            get
            {
                float norm = this.w * this.w + this.x * this.x + this.y * this.y + this.z * this.z;
                if (norm > 0.0f)
                {
                    float inverseNorm = 1.0f / norm;
                    return new Quaternion(this.w * inverseNorm, -this.x * inverseNorm, -this.y * inverseNorm, -this.z * inverseNorm);
                }
                else
                {
                    // return an invalid result to flag the error
                    return Quaternion.Zero;
                }
            }
        }

        /// <summary>
        /// Variant of Inverse (called conjugate) that is only valid for unit quaternions.
        /// The "conjugate" of a quaternion is the same as the inverse, as long as the quaternion is unit-length
        /// </summary>
        public Quaternion UnitInverse
        {
            get
            {
                return new Quaternion(w, -x, -y, -z);
            }
        }

        #endregion

        #region Static methods

        public static Quaternion Slerp(float time, Quaternion quatA, Quaternion quatB)
        {
            return Slerp(time, quatA, quatB, false);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="time"></param>
        /// <param name="quatA"></param>
        /// <param name="quatB"></param>
        /// <param name="useShortestPath"></param>
        /// <returns></returns>
        public static Quaternion Slerp(float time, Quaternion quatA, Quaternion quatB, bool useShortestPath)
        {
            float cos = quatA.Dot(quatB);

            float angle = MathHelper.ACos(cos);

            if (System.Math.Abs(angle) < EPSILON)
            {
                return quatA;
            }

            float sin = (float)System.Math.Sin(angle);
            float inverseSin = 1.0f / sin;
            float coeff0 = (float)System.Math.Sin((1.0f - time) * angle) * inverseSin;
            float coeff1 = (float)System.Math.Sin(time * angle) * inverseSin;

            Quaternion result;

            if (cos < 0.0f && useShortestPath)
            {
                coeff0 = -coeff0;
                // taking the complement requires renormalisation
                Quaternion t = coeff0 * quatA + coeff1 * quatB;
                t.Normalize();
                result = t;
            }
            else
            {
                result = (coeff0 * quatA + coeff1 * quatB);
            }

            return result;
        }

        /// <overloads><summary>
        /// normalised linear interpolation - faster but less accurate (non-constant rotation velocity)
        /// </summary>
        /// <param name="fT"></param>
        /// <param name="rkP"></param>
        /// <param name="rkQ"></param>
        /// <returns></returns>
        /// </overloads>
        public static Quaternion Nlerp(float fT, Quaternion rkP, Quaternion rkQ)
        {
            return Nlerp(fT, rkP, rkQ, false);
        }


        /// <param name="shortestPath"></param>
        public static Quaternion Nlerp(float fT, Quaternion rkP, Quaternion rkQ, bool shortestPath)
        {
            Quaternion result;
            float fCos = rkP.Dot(rkQ);
            if (fCos < 0.0f && shortestPath)
            {
                result = rkP + fT * ((-rkQ) - rkP);
            }
            else
            {
                result = rkP + fT * (rkQ - rkP);

            }
            result.Normalize();
            return result;
        }

        public static Quaternion Squad(float t, Quaternion p, Quaternion a, Quaternion b, Quaternion q)
        {
            return Squad(t, p, a, b, q, false);
        }

        /// <summary>
        ///		Performs spherical quadratic interpolation.
        /// </summary>
        /// <param name="t"></param>
        /// <param name="p"></param>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="q"></param>
        /// <returns></returns>
        public static Quaternion Squad(float t, Quaternion p, Quaternion a, Quaternion b, Quaternion q, bool useShortestPath)
        {
            float slerpT = 2.0f * t * (1.0f - t);

            // use spherical linear interpolation
            Quaternion slerpP = Slerp(t, p, q, useShortestPath);
            Quaternion slerpQ = Slerp(t, a, b);

            // run another Slerp on the results of the first 2, and return the results
            return Slerp(slerpT, slerpP, slerpQ);
        }

        #endregion

        #region Euler Angles

        public Vector3 ToEulerAnglesInDegrees()
        {
            float pitch, yaw, roll;
            ToEulerAngles(out pitch, out yaw, out roll);
            return new Vector3(MathHelper.RadiansToDegrees(pitch), MathHelper.RadiansToDegrees(yaw), MathHelper.RadiansToDegrees(roll));
        }

        public Vector3 ToEulerAngles()
        {
            float pitch, yaw, roll;
            ToEulerAngles(out pitch, out yaw, out roll);
            return new Vector3(pitch, yaw, roll);
        }

        public void ToEulerAnglesInDegrees(out float pitch, out float yaw, out float roll)
        {
            ToEulerAngles(out pitch, out yaw, out roll);
            pitch = MathHelper.RadiansToDegrees(pitch);
            yaw = MathHelper.RadiansToDegrees(yaw);
            roll = MathHelper.RadiansToDegrees(roll);
        }

        public void ToEulerAngles(out float pitch, out float yaw, out float roll)
        {

            float halfPi = MathHelper.PI / 2;
            float test = x * y + z * w;
            if (test > 0.499f)
            { // singularity at north pole
                yaw = 2 * (float)System.Math.Atan2(x, w);
                roll = halfPi;
                pitch = 0;
            }
            else if (test < -0.499f)
            { // singularity at south pole
                yaw = -2 * (float)System.Math.Atan2(x, w);
                roll = -halfPi;
                pitch = 0;
            }
            else
            {
                float sqx = x * x;
                float sqy = y * y;
                float sqz = z * z;
                yaw = (float)System.Math.Atan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz);
                roll = (float)System.Math.Asin(2 * test);
                pitch = (float)System.Math.Atan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz);
            }

            if (pitch <= float.Epsilon)
                pitch = 0f;
            if (yaw <= float.Epsilon)
                yaw = 0f;
            if (roll <= float.Epsilon)
                roll = 0f;
        }

        public static Quaternion FromEulerAnglesInDegrees(float pitch, float yaw, float roll)
        {
            return FromEulerAngles(MathHelper.DegreesToRadians(pitch), MathHelper.DegreesToRadians(yaw), MathHelper.DegreesToRadians(roll));
        }

        /// <summary>
        /// Combines the euler angles in the order yaw, pitch, roll to create a rotation quaternion
        /// </summary>
        /// <param name="pitch"></param>
        /// <param name="yaw"></param>
        /// <param name="roll"></param>
        /// <returns></returns>
        public static Quaternion FromEulerAngles(float pitch, float yaw, float roll)
        {
            return Quaternion.FromAngleAxis(Vector3.UnitY, yaw)
                * Quaternion.FromAngleAxis(Vector3.UnitX, pitch)
                * Quaternion.FromAngleAxis(Vector3.UnitZ, roll);

            /*TODO: Debug
            //Equation from http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
            //heading
			
            float c1 = (float)Math.Cos(yaw/2);
            float s1 = (float)Math.Sin(yaw/2);
            //attitude
            float c2 = (float)Math.Cos(roll/2);
            float s2 = (float)Math.Sin(roll/2);
            //bank
            float c3 = (float)Math.Cos(pitch/2);
            float s3 = (float)Math.Sin(pitch/2);
            float c1c2 = c1*c2;
            float s1s2 = s1*s2;

            float w =c1c2*c3 - s1s2*s3;
            float x =c1c2*s3 + s1s2*c3;
            float y =s1*c2*c3 + c1*s2*s3;
            float z =c1*s2*c3 - s1*c2*s3;
            return new Quaternion(w,x,y,z);*/
        }

        #endregion

        #region Dot

        /// <summary>
        /// Performs a Dot Product operation on 2 Quaternions.
        /// </summary>
        public float Dot(Quaternion quat)
        {
            return this.w * quat.w + this.x * quat.x + this.y * quat.y + this.z * quat.z;
        }

        #endregion

        #region Normalize

        /// <summary>
        ///	Normalizes elements of this quaterion to the range [0,1].
        /// </summary>
        public void Normalize()
        {
            float factor = 1f / (float)System.Math.Sqrt(this.Norm);

            w = w * factor;
            x = x * factor;
            y = y * factor;
            z = z * factor;
        }

        #endregion

        #region ToRotationMatrix

        /// <summary>
        /// Gets a 3x3 rotation matrix from this Quaternion.
        /// </summary>
        public Matrix3 ToRotationMatrix()
        {
            Matrix3 rotation = Matrix3.Identity.Clone();

            float length = (x * x) + (y * y) + (z * z) + (w * w);
            float s = (length > 0f) ? 2f / length : 0f;

            float xx = x * x;
            float yy = y * y;
            float zz = z * z;

            float xw = x * w;
            float yw = y * w;
            float zw = z * w;

            float xz = x * z;
            float xy = x * y;

            float yz = y * z;

            rotation.M00 = 1f - (s * yy + s * zz);
            rotation.M10 = s * xy - s * zw;
            rotation.M20 = s * xz + s * yw;
            rotation.M01 = s * xy + s * zw;
            rotation.M11 = 1f - (s * xx + s * zz);
            rotation.M21 = s * yz - s * xw;
            rotation.M02 = s * xz - s * yw;
            rotation.M12 = s * yz + s * xw;
            rotation.M22 = 1f - (s * xx + s * yy);

            return rotation;
        }

        #endregion

        #region ToRotationMatrix : with pivot point

        // Rotation about a point other than origin
        // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
        /*
        void setRotate(sfquat q, sfvec3f centre)
        {
            float sqw = q.w * q.w;
            float sqx = q.x * q.x;
            float sqy = q.y * q.y;
            float sqz = q.z * q.z;
            m00 = sqx - sqy - sqz + sqw; // since sqw + sqx + sqy + sqz =1
            m11 = -sqx + sqy - sqz + sqw;
            m22 = -sqx - sqy + sqz + sqw;

            float tmp1 = q.x * q.y;
            float tmp2 = q.z * q.w;
            m01 = 2.0 * (tmp1 + tmp2);
            m10 = 2.0 * (tmp1 - tmp2);

            tmp1 = q.x * q.z;
            tmp2 = q.y * q.w;
            m02 = 2.0 * (tmp1 - tmp2);
            m20 = 2.0 * (tmp1 + tmp2);

            tmp1 = q1.y * q.z;
            tmp2 = q1.x * q.w;
            m12 = 2.0 * (tmp1 + tmp2);
            m21 = 2.0 * (tmp1 - tmp2);

            float a1, a2, a3;
            if (centre == null)
            {
                a1 = a2 = a3 = 0;
            }
            else
            {
                a1 = centre.x;
                a2 = centre.y;
                a3 = centre.z;
            }
            m03 = a1 - a1 * m00 - a2 * m01 - a3 * m02;
            m13 = a2 - a1 * m10 - a2 * m11 - a3 * m12;
            m23 = a3 - a1 * m20 - a2 * m21 - a3 * m22;
            m30 = m31 = m32 = 0.0;
            m33 = 1.0;
        }*/

        #endregion

        #region FromRotationMatrix

        public void FromRotationMatrix(Matrix3 matrix)
        {
            // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
            // article "Quaternion Calculus and Fast Animation".

            float trace = matrix.M00 + matrix.M11 + matrix.M22;

            float root = 0.0f;

            if (trace > 0.0f)
            {
                // |this.w| > 1/2, may as well choose this.w > 1/2
                root = (float)System.Math.Sqrt(trace + 1.0f);  // 2w
                this.w = 0.5f * root;

                root = 0.5f / root;  // 1/(4w)

                this.x = (matrix.M21 - matrix.M12) * root;
                this.y = (matrix.M02 - matrix.M20) * root;
                this.z = (matrix.M10 - matrix.M01) * root;
            }
            else
            {
                // |this.w| <= 1/2

                int i = 0;
                if (matrix.M11 > matrix.M00)
                    i = 1;
                if (matrix.M22 > matrix[i, i])
                    i = 2;

                int j = next[i];
                int k = next[j];

                root = (float)System.Math.Sqrt(matrix[i, i] - matrix[j, j] - matrix[k, k] + 1.0f);

                unsafe
                {
                    fixed (float* apkQuat = &this.x)
                    {
                        apkQuat[i] = 0.5f * root;
                        root = 0.5f / root;

                        this.w = (matrix[j, k] - matrix[k, j]) * root;

                        apkQuat[j] = (matrix[i, j] + matrix[j, i]) * root;
                        apkQuat[k] = (matrix[i, k] + matrix[k, i]) * root;
                    }
                }
            }
        }

        #endregion

        #region ToAngleAxis

        /// <summary>
        ///    
        /// </summary>
        /// <param name="angle"></param>
        /// <param name="axis"></param>
        /// <returns></returns>
        public void ToAngleAxis(ref float angle, ref Vector3 axis)
        {
            // The quaternion representing the rotation is
            //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

            float sqrLength = x * x + y * y + z * z;

            if (sqrLength > 0.0f)
            {
                angle = 2.0f * MathHelper.ACos(w);
                float invLength = MathHelper.InvSqrt(sqrLength);
                axis.x = x * invLength;
                axis.y = y * invLength;
                axis.z = z * invLength;
            }
            else
            {
                angle = 0.0f;
                axis.x = 1.0f;
                axis.y = 0.0f;
                axis.z = 0.0f;
            }
        }

        #endregion

        #region ToAxes

        /// <summary>
        /// Convert the quaternion to axis.
        /// </summary>
        public void ToAxes(out Vector3 xAxis, out Vector3 yAxis, out Vector3 zAxis)
        {
            xAxis = new Vector3();
            yAxis = new Vector3();
            zAxis = new Vector3();

            Matrix3 rotation = this.ToRotationMatrix();

            xAxis.x = rotation.M00;
            xAxis.y = rotation.M10;
            xAxis.z = rotation.M20;

            yAxis.x = rotation.M01;
            yAxis.y = rotation.M11;
            yAxis.z = rotation.M21;

            zAxis.x = rotation.M02;
            zAxis.y = rotation.M12;
            zAxis.z = rotation.M22;
        }

        #endregion

        #region FromAngleAxis

        /// <summary>
        /// Creates a Quaternion from a supplied angle and axis.
        /// </summary>
        /// <param name="axis">Arbitrary axis vector.</param>
        /// <param name="angle">Value of an angle in radians.</param>
        public static Quaternion FromAngleAxis(Vector3 axis, float angle)
        {
            Quaternion quat = new Quaternion();

            axis.Normalize();

            float halfAngle = 0.5f * angle;
            float sin = (float)System.Math.Sin(halfAngle);

            quat.w = (float)System.Math.Cos(halfAngle);
            quat.x = sin * axis.x;
            quat.y = sin * axis.y;
            quat.z = sin * axis.z;

            return quat;
        }

        #endregion

        #region Log

        /// <summary>
        ///	Calculates the logarithm of a Quaternion.
        /// </summary>
        public Quaternion Log()
        {
            // BLACKBOX: Learn this
            // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
            // log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
            // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.

            // start off with a zero quat
            Quaternion result = Quaternion.Zero;

            if (System.Math.Abs(w) < 1.0f)
            {
                float angle = MathHelper.ACos(w);
                float sin = (float)System.Math.Sin(angle);

                if (System.Math.Abs(sin) >= EPSILON)
                {
                    float coeff = angle / sin;
                    result.x = coeff * x;
                    result.y = coeff * y;
                    result.z = coeff * z;
                }
                else
                {
                    result.x = x;
                    result.y = y;
                    result.z = z;
                }
            }

            return result;
        }

        #endregion

        #region Exp

        /// <summary>
        ///	Calculates the Exponent of a Quaternion.
        /// </summary>
        public Quaternion Exp()
        {
            // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
            // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
            // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.

            float angle = (float)System.Math.Sqrt(x * x + y * y + z * z);
            float sin = (float)System.Math.Sin(angle);

            // start off with a zero quat
            Quaternion result = Quaternion.Zero;

            result.w = (float)System.Math.Cos(angle);

            if (System.Math.Abs(sin) >= EPSILON)
            {
                float coeff = sin / angle;

                result.x = coeff * x;
                result.y = coeff * y;
                result.z = coeff * z;
            }
            else
            {
                result.x = x;
                result.y = y;
                result.z = z;
            }

            return result;
        }

        #endregion

        #region Clone

        public Quaternion Clone()
        {
            return new Quaternion(w, x, y, z);
        }

        #endregion

        #region Misc

        public override string ToString()
        {
            return string.Format("Quaternion({0}, {1}, {2}, {3})", this.x, this.y, this.z, this.w);
        }

        public override int GetHashCode()
        {
            return (int)x ^ (int)y ^ (int)z ^ (int)w;
        }

        public override bool Equals(object obj)
        {
            Quaternion quat = (Quaternion)obj;

            return quat == this;
        }

        public bool Equals(Quaternion rhs, float tolerance)
        {
            float fCos = Dot(rhs);
            float angle = MathHelper.ACos(fCos);

            return System.Math.Abs(angle) <= tolerance;
        }

        #endregion

    }

}
